#include "edges.hpp"
#include "mesh.hpp"
#include "meshFunctions.hpp"
#include "geometry.hpp"
#include "signalCodes.hpp"
#include "topology.hpp"
#include "macros.hpp"
#include <algorithm>
#include <cassert>
#include <iostream>
#include <utility>


namespace hms
{

Edges::Edges( const Mesh& mesh ) :
	EdgesBase<Edges>(mesh)
{}


#ifdef STRUCTMESH_PRECALC_EDGE_PARENT_CELLS
auto Edges::cellsImp(Index) const -> const Array2Xi& {
#else
auto Edges::cells() const -> const Array2Xi& {
#endif
	return m_cells;
}

auto Edges::vertices() const -> const Array2Xi& {
	return m_vertices;
}

auto Edges::lengthsImp() const -> const ArrayXs& {
	return m_lengths;
}

auto Edges::normalsImp() const -> const Matrix2Xs& {
	return m_normals;
}

void Edges::nInnerEdges(Index n){
	assert( m_nInnerEdges == signalCode::uninitialised );
	m_nInnerEdges = n;
}
void Edges::vertices( Array2Xi&& verts ){
	assert( m_vertices.size() == 0 && "Vertices already set!" );
	m_vertices = std::move(verts);
	storeLengths();
}
void Edges::cells( Array2Xi&& cellIndices ){
	assert( m_cells.size() == 0 && "Cell indices already set!" );
	m_cells = std::move(cellIndices);
}

void Edges::storeData(
	Index nInner,
	Array2Xi&& vertexIndices,
	Array2Xi&& cellsArg
){
	nInnerEdges( nInner );
	vertices( std::move(vertexIndices) );
	cells( std::move(cellsArg) );
}



// auto Edges::mesh() const -> const Mesh& {
// 	assert ( m_mesh && "Edges object does not have a mesh assigned to it! Dereferencing nullptr!" );
// 	return *m_mesh;
// }


auto Edges::nEdges() const -> Index {
	return m_vertices.cols();
}

auto Edges::nInnerEdges() const -> Index {
	assert( m_nInnerEdges != signalCode::uninitialised || nEdges() == 0 );
	return m_nInnerEdges;
}

auto Edges::nBoundaryEdges() const -> Index {
	assert( m_nInnerEdges != signalCode::uninitialised );
	return nEdges() - nInnerEdges();
}

#define ASSERT_INDEX(MAX) assert( \
	edge >= 0 && edge < MAX && "Invalid edge index!" )

auto Edges::length(Index edge) const -> scalar {
	ASSERT_INDEX( m_lengths.size() );
	return m_lengths[edge];
}
auto Edges::vertices(Index edge) const -> Traits::VertexIndices {
	ASSERT_INDEX( m_vertices.cols() );
	return m_vertices.col(edge);
}
/* returns the cell indices for a single edge */
auto Edges::cells_s(Index edge) const -> Traits::CellIndices {
	ASSERT_INDEX( m_cells.cols() );
	return m_cells.col(edge);
}
auto Edges::normal     (Index edge) const -> Traits::NormalVector {
	ASSERT_INDEX( m_normals.cols() );
	return m_normals.col(edge);
}
auto Edges::midpoint  (Index edge) const -> Vector2s {
	ASSERT_INDEX( m_vertices.cols() );
	/* average of indexed view of the vertices belonging to the edge */
	return mesh().vertices()(hms::all, m_vertices.col(edge))
		.rowwise().sum() / 2;
}
auto Edges::intersection(Index edge) const -> Vector2s {
	ASSERT_INDEX( nEdges() );
	return intersectionWithCentroids( mesh().edge(edge) );
}
auto Edges::isBoundary(Index edge) const -> bool {
	ASSERT_INDEX( nEdges() );
	return edge >= m_nInnerEdges;
}


/**
 * @brief If the inquired edge is a boundary edge, this function returns the 
 * index of the domain cell it belongs to. In fact, this function simply returns
 * the first of its two parent cells, i.e. the one with a lower index. For
 * boundary edges, this is the domain cell.
 * @see Edges::ghostCell()
 */
auto Edges::domainCell(Index edge) const -> Index {
	ASSERT_INDEX( nEdges() );
	assert( isBoundary(edge) &&
		"Edges::domainCell() is for boundary edges only!" );
	
	return this->cells_s(edge)[0];
}

/**
 * @brief If the edge is a boundary edge, this function returns the index of
 * the ghost cell it belongs to. In fact, this function simply returns the
 * second of its two parent cells, i.e. the one with a higher index. For 
 * boundary edges, this is the ghost cell.
 * @see Edges::domainCell()
 */
auto Edges::ghostCell(Index edge) const -> Index {
	ASSERT_INDEX( nEdges() );
	assert( isBoundary(edge) &&
		"Edges::ghostCell() is for boundary edges only!" );
	
	return cells_s(edge)[1];
}




/**
 * @brief Swaps all relevant data for two edges A and B. Does not access cells,
 * so it should not be called by itself, but rather through the appropriate
 * mesh interface
 * @param edgeA
 * @param edgeB
 */
void Edges::swap(Index edgeA, Index edgeB){
	assert( edgeA >= 0 && edgeA < nEdges() && edgeB >= 0 && edgeB < nEdges() );
	
	m_vertices.col(edgeA).swap( m_vertices.col(edgeB) );
	m_cells.col(edgeA).swap( m_cells.col(edgeB) );
	
	std::swap( m_lengths(edgeA), m_lengths(edgeB) );
	
	if (m_normals.cols() != 0)
		m_normals.col(edgeA).swap( m_normals.col(edgeB) );
}


// /**
//  * @brief Reorders edges in a way that boundary edges are in the back, i.e. 
//  * their indices start at nInnerEdges.
//  * @return all swapped indices.
//  */
// auto Edges::boundaryToBack() -> std::vector<Array2i> {
// 	/* to swap inner with boundary edges, we first need to find the boundary
// 	 * edges (by checking their parent cells) */
// 	IdxVector boundaryEdges { findEdgesWEmptyParent(m_cells) };
// 	assert( std::is_sorted( boundaryEdges.begin(), boundaryEdges.end() ) );
	
// 	/* given that whether or not an edge is a boundary edge is to be decided
// 	 * only by its index, the number of inner edges must be stored */
// 	m_nInnerEdges = nEdges() - static_cast<Index>( boundaryEdges.size() );
	
// 	/* boundaryEdges is sorted, so the number of boundary edges that must be
// 	 * swapped can be determined by finding the first edge index higher than the
// 	 * number of inner edges. All edge indices after that one are larger and
// 	 * therefore already occupy a boundary edge position. */
// 	Index nEdgesToSwap { std::lower_bound(
// 		boundaryEdges.begin(), boundaryEdges.end(), m_nInnerEdges ) -
// 		boundaryEdges.begin()
// 	};
	
// 	IdxVector innerEdgesToSwap;
// 	innerEdgesToSwap.reserve( nEdgesToSwap );
	
// 	/* start at the first edge index that should represent a boundary edge and
// 	 * check whether that or higher indices represent inner edges */
// 	for ( Index i {m_nInnerEdges}; i<nEdges(); ++i )
// 		if ( !hasEmptyParent(m_cells, i) )
// 			innerEdgesToSwap.push_back(i);
	
// 	/* swapping always needs two edges, so the amount of inner and boundary 
// 	 * edges to swap must be the same */
// 	assert( innerEdgesToSwap.size() == static_cast<uIndex>(nEdgesToSwap) );
	
// 	std::vector<Array2i> swapped ( nEdgesToSwap );
	
// 	for ( Index i=0; i<nEdgesToSwap; ++i ){
// 		this->swap(boundaryEdges[i], innerEdgesToSwap[i]);
// 		swapped[i] = Array2i{ boundaryEdges[i], innerEdgesToSwap[i] };
// 		/* update boundary edge indices */
// 		boundaryEdges[i] = innerEdgesToSwap[i];
// 	}
	
// 	return swapped;
// }



/**
 * @brief Calculates the length of each edge by accessing the endpoints through
 * the parent mesh. Then stores these in the m_lengths member variable.
 */
void Edges::storeLengths() {
	assert( m_lengths.size() == 0 );
	/* as many lengths as edges */
	m_lengths.resize( nEdges() );

	/* embarassingly parallel */
	OMP_PRAGMA(parallel for schedule(static))
	for ( int i=0; i < m_lengths.size() ; ++i ) {
		m_lengths[i] = (
			mesh().vertex(m_vertices.col(i)[0]) -
			mesh().vertex(m_vertices.col(i)[1])
		).norm();
	}
}



/**
 * @brief Calculates the unit normal vector of each edge and stores it in the 
 * m_normals member variable. Each normal vector points from the parent cell
 * with a lower index to that with a higher index
 */
void Edges::storeNormals()
{
	assert( m_normals.size() == 0 );
	/* as many normal vectors as edges */
	m_normals.resize( NoChange, nEdges() );
	
	OMP_PRAGMA(parallel for schedule(static))
	for (Index i=0; i < nEdges(); ++i){
		/* the normal() method always produces a leftward pointing normal 
		 * vector. However, to decouple the orientation from vertex indices,
		 * the convention is that the stored normal vector points from the
		 * parent cell with a lower index into the one with a higher index */
		Vector2s normal { Line::Through(
			mesh().vertex(m_vertices.col(i)[0]),
			mesh().vertex(m_vertices.col(i)[1])
		).normal() };
		
		/* determine on which Side of the edge the centroid of the first parent
		 * cell (i.e. the one with a lower index) lies. If it lies to the right,
		 * the leftward pointing normal vector can be stored as-is, otherwise
		 * it needs to be flipped */
		m_normals.col(i) = 
			whichSide(
				mesh().vertex(m_vertices.col(i)[0]),
				mesh().vertex(m_vertices.col(i)[1]),
				mesh().cells().centroid(m_cells.col(i)[0])
			) == Side::right ?
			normal : -normal;
	}
}

}

#undef ASSERT_INDEX